#include <fstream>
#include <iostream>
#include <vector>

#include "data_storage.hpp"
#include "gnss_file_loader.hpp"
#include "imu_file_loader.hpp"
#include "truth_file_loader.hpp"

#include "initial_align.hpp"

using namespace std;
using namespace iNav;

string read_path = "/home/ubuntu/Dataset/20201219-操场T17/";
string imu_data_name = "IMU_HL_INSPROBE_11_IMU.bin";
string gnss_data_name = "GNSS_HL_INSPROBE_11_GNSS.txt";
string truth_name = "REF_LC_SM_TXT_to_T17.nav";

string output_name = "result_interrupt.nav";

vector<double> change_time_vec = {5.28E5 + 1138};
double static_time_length = 20;
double dynamic_time_length = 1;

int main(int, char**) {
    // read files
    IMUFileLoader imu_loader(read_path + imu_data_name);
    GNSSFileLoader gnss_loader(read_path + gnss_data_name);
    TruthFileLoader truth_loader(read_path + truth_name);

    vector<IMUData> static_imu_vec, dynamic_imu_vec;
    vector<GnssData> static_gnss_vec;
    GnssData final_gnss_pos;
    Truth final_truth;

    for (auto imu_data : imu_loader.m_imu_vec) {
        for (auto time : change_time_vec) {
            if (imu_data.timestamp >= time - static_time_length && imu_data.timestamp <= time)
                static_imu_vec.push_back(imu_data);

            if (imu_data.timestamp > time && imu_data.timestamp <= time + dynamic_time_length)
                dynamic_imu_vec.push_back(imu_data);
        }
    }

    for (auto gnss_data : gnss_loader.m_gnss_vec) {
        for (auto time : change_time_vec) {
            if (gnss_data.timestamp >= time - static_time_length && gnss_data.timestamp <= time)
                static_gnss_vec.push_back(gnss_data);

            if (gnss_data.timestamp == time + dynamic_time_length)
                final_gnss_pos = gnss_data;
        }
    }

    for (auto truth_data : truth_loader.m_truth_vec) {
        for (auto time : change_time_vec) {
            if (truth_data.second == time + dynamic_time_length)
                final_truth = truth_data;
        }
    }

    InitialAlign initial_align_test(static_imu_vec, static_gnss_vec);
    double yaw = initial_align_test.CalculateYaw(dynamic_imu_vec, final_gnss_pos);

    cout << yaw << "      "<< final_truth.att[2] << endl;

    return 0;
}
